#pragma once
#include <gl\gl.h>
#include <gl\glu.h>


class RoboArm
{
private:
	GLfloat m_a1;
	GLfloat m_a2;
	GLint m_d1;
	GLint m_d2;
	float ballTargetAngle[2];
	RoboArm() {}	

public:
		
	RoboArm(GLfloat a1, GLfloat a2, GLint d1, GLint d2)
	{
		m_a1 = a1;
		m_a2 = a2;
		m_d1 = d1;
		m_d2 = d2;
		ballTargetAngle[0] = 0.0f;
		ballTargetAngle[1] = 0.0f;
	}

	void Draw(GLUquadric* quadric)
	{
		glPushMatrix();
		glColor3f(1, 0, 0);
		glRotatef(m_a1, 0, 0, 1);
		glRotatef(90, 0, 1, 0);
		gluCylinder(quadric, 1, 1, m_d1, 8, 2);
		glPopMatrix();

		glPushMatrix();

		glColor3f(0, 0, 1);
		glRotatef(m_a1, 0, 0, 1);
		glTranslatef(m_d1, 0, 0);

		glRotatef(m_a2, 0, 0, 1);
		glRotatef(90, 0, 1, 0);

		gluCylinder(quadric, 1, 1, m_d2, 8, 2);

		glPopMatrix();
	}

	void Update(Vertex3F *ballLoc)
	{
		float temp = sqrt( pow(ballLoc -> x, 2) + pow(ballLoc -> y, 2));

		if(temp != 0 && temp < (m_d1 + m_d2))
		{
			float n_a1 = atan2f(ballLoc -> y, ballLoc -> x) * (float)180 / PI;
			float a1Temp = acosf(temp * 0.5f / m_d1) * (float)180 / PI;
			float n_a2 = (270 - a1Temp) * 2;

			ballTargetAngle[0] = n_a1 + a1Temp;
			ballTargetAngle[1] = n_a2 + 180;

			m_a1 += (ballTargetAngle[0] - m_a1) * 0.01f;
			m_a2 += (ballTargetAngle[1] - m_a2) * 0.01f;
		}
	}
};